/**
  @file bucket_transport_task.cpp
 * @brief 实现条筒抓取与放置任务和与调度系统进行协作
 * @author weeksun
 * @version 1.0
 * @date 2017-10-01
 */

#include "bucket_transport_task.h"

using namespace AgvAssistant;

BucketTransport::BucketTransport(RobotDriverBasePtr& driver, std::string name):
  robot_driver_ptr_(driver),
  nh_(),
  nh_private_("~"),
  task_name_(name){

  nh_bucket_transport_ = ros::NodeHandle(nh_private_,task_name_);

  bucket_sub_ = nh_bucket_transport_.subscribe("/bucket_pose",20,&BucketTransport::BucketDetectionCallback,this);

  camera_sub_ = nh_.subscribe("/camera_bucket",20,&BucketTransport::CamDetectionInfoCallback,this);

  camera_control_pub_ = nh_.advertise<std_msgs::Int32>("/camera_control", 20);

  laser_detection_reset_pub_ = nh_.advertise<std_msgs::Int32>("/detection_reset",20);
  /** 注册相关回调函数 */
  robot_driver_ptr_->registerWorldPoseCallback(
        boost::bind(&BucketTransport::worldPoseCallback,this,_1));
  robot_driver_ptr_->registerGroupControlCallback(
        boost::bind(&BucketTransport::groupControlCallback,this,_1));
  robot_driver_ptr_->registerPoseAdjustAckCallback(
        boost::bind(&BucketTransport::poseAdjustAckCallback,this,_1));
  robot_driver_ptr_->registerOdomCallback(
        boost::bind(&BucketTransport::robotOdomCallback,this,_1));
  robot_driver_ptr_->registerWorkAreaCallback(
        boost::bind(&BucketTransport::workAreaCallback,this,_1));
  robot_driver_ptr_->registerEmergentCallback(
        boost::bind(&BucketTransport::emergentCallback,this,_1));
  robot_driver_ptr_->registerRobotLaserPoseCallback(
        boost::bind(&BucketTransport::laserPoseCallback,this,_1));


  pose_.setIdentity();
  straight2pre_.setIdentity();

  getParam();

  working_th_ = boost::thread(boost::bind(&BucketTransport::working_exec,this));

}

BucketTransport::~BucketTransport(){
   working_th_.join();
}

void BucketTransport::getParam(){
  max_vel_x_ = nh_bucket_transport_.param("max_vel_x",0.5);
  max_vel_y_ = nh_bucket_transport_.param("max_vel_y",0.5);
  max_vel_th_ = nh_bucket_transport_.param("max_vel_th",1.3);
  precision_x_ = nh_bucket_transport_.param("precision_x",0.1);
  precision_y_ = nh_bucket_transport_.param("precision_y",0.1);
  precision_th_ = nh_bucket_transport_.param("precision_th",0.02);
  distance_bucket_ = nh_bucket_transport_.param("distance_bucket",1.3);
  back_distance_ = nh_bucket_transport_.param("back_distance",-0.5);
  precapture_angle_ = nh_bucket_transport_.param("precapture_angle",0.523);
  precapture_radius_ = nh_bucket_transport_.param("precapture_radius",0.3);
  target_radius_ = nh_bucket_transport_.param("target_object_radius",0.25);
  avoid_obstacle_width_ = nh_bucket_transport_.param("avoid_obstacle_width",0.3);
  avoid_obstacle_distance_ = nh_bucket_transport_.param("avoid_obstacle_distance",1.0);
  laser2baselink_ = nh_bucket_transport_.param("laser2baselink",0.15);
  camera2bucket_dis_ = nh_bucket_transport_.param("camera2bucket_dis",0.5);
  bucketPut_distance_ = nh_bucket_transport_.param("bucketPut_distance",0.5);
  bucket_radius_ = nh_bucket_transport_.param("target_object_radius",0.25);
  put_data_.robot_bucket_dis = nh_bucket_transport_.param("robot_bucket_dis",1.5);
  point_back_dis_ = nh_bucket_transport_.param("point_back_dis",-0.3);
  camera_adjust_ = nh_bucket_transport_.param("camera_adjust",1);
}

void BucketTransport::CamDetectionInfoCallback(const boost::shared_ptr<geometry_msgs::PoseArray>& msg){
  
   boost::unique_lock<boost::mutex> lock(data_update_mutex_);

   if(msg->poses.size() == 2){
     
    // ROS_INFO("circle_center_point_.position.y:::%f",msg->poses[0].position.y);
     circle_center_point_.position.x = msg->poses[0].position.x;
     circle_center_point_.position.y = msg->poses[0].position.y;
     circle_center_point_.position.z = 0;
    //ROS_INFO("nearest_point_.position.x:::%f,,y:::%f",msg->poses[1].position.x,msg->poses[1].position.y);

     nearest_point_.position.x = msg->poses[1].position.x;//dis
     nearest_point_.position.y = msg->poses[1].position.y;//left
     nearest_point_.position.z = 0;
   }
}

void BucketTransport::BucketDetectionCallback(const boost::shared_ptr<geometry_msgs::PoseArray>& bucket_poses){
  boost::unique_lock<boost::mutex> lock(data_update_mutex_);
  bucket_points_.clear();
  tf::Vector3 bucketPose;
  for(int i = 0;i<bucket_poses->poses.size();i++)
  {
    bucketPose.setX(bucket_poses->poses[i].position.x);
    bucketPose.setY(bucket_poses->poses[i].position.y);
    bucketPose.setZ(0);
    bucket_points_.push_back(bucketPose);
  }
//  ROS_INFO("BUCKET POSE......");
}

void BucketTransport::worldPoseCallback(const geometry_msgs::Pose& pose){
  //ROS_INFO("Global pose.....");
  boost::unique_lock<boost::mutex> lock(data_update_mutex_);
  tf::poseMsgToTF(pose,global_pose_);
}

void BucketTransport::laserPoseCallback(const std::vector<tf::Vector3>& points){
  boost::unique_lock<boost::mutex> lock(data_update_mutex_);
  points_robot_ = points;
}

void BucketTransport::robotOdomCallback(const nav_msgs::OdometryConstPtr& msg){
  //ROS_INFO("Odom recv......");
}

void BucketTransport::poseAdjustAckCallback(const RobotDriverBase::PoseAckInfo& ack){
  boost::unique_lock<boost::mutex> lock(data_update_mutex_);
  pose_ack_ = ack;
  recv_pose_ack_ = true;
  ROS_INFO("Pose Adj ack....");
}

void BucketTransport::groupControlCallback(const RobotDriverBase::GroupControlInfo& ctl){
  ROS_INFO("Groupctl.... %s",ctl.evt_str.c_str());
  YAML::Node node = YAML::Load(ctl.evt_str);
  if(node["task"].as<std::string>() == "getBucketTask"){
    if(node["type"].as<int>() == 0){
      requestGetTask(ctl,node["id"].as<int>());  
      point_search_type_ = false;
    }
    else if(node["type"].as<int>() == 1){
      task_type_ = GET_TASK;
      get_data_.main_status = GetTaskData::POINT_SEARCH;
      point_search_type_ = true;
    }
    ctl_ = ctl;
  }
  else if(node["task"].as<std::string>() == "putBucketTask"){
    if(node["type"].as<int>() == 0){
      requestPutTask(ctl,node["id"].as<int>());
    }
    else if(node["type"].as<int>() == 1){
      task_type_ = PUT_TASK;
      put_data_.main_status = PutTaskData::POINT_PUT;
    }
    ctl_ = ctl;
  }
  else if(node["task"].as<std::string>() == "resetArea"){
    reset_getarea_ = true;
    reset_putarea_ = true;
    robot_driver_ptr_->sendGroupControlAck(ctl);
  }
}

void BucketTransport::emergentCallback(const RobotDriverBase::EmergentInfo& info){
  ROS_INFO("Emergent....");
  boost::unique_lock<boost::mutex> lock(data_update_mutex_);
  // reset_getarea_ = true;
  // reset_putarea_ = true;
  if(task_type_ == NO_TASK){
    emergent_info_ = false;
  }
  else{
    emergent_info_ = true;
  }
}

void BucketTransport::workAreaCallback(const WorkArea& area){
  float Hot_area_len = nh_bucket_transport_.param("hot_area_len",0.8);
  float Target_object_radius = nh_bucket_transport_.param("target_object_radius",0.25);
  float Distance_of_expandsion = nh_bucket_transport_.param("distance_of_expandsion",1.5);
  
  AgvAssistant::AreaManager area_set(Hot_area_len,Target_object_radius,Distance_of_expandsion);

  std::cout<<"task:::"<<area.task<<std::endl;
    laser_detection_reset_.data = 1;
    laser_detection_reset_pub_.publish(laser_detection_reset_);
    if(area.task == "set_getBucketArea" && area.type == 0)
    {

      p1_ = area.area[0];
      p3_ = area.area[2];
      if(reset_getarea_ == true){
        area_set.generateArea(area.seq,AreaManager::AREA_GET,area.area,area.dock);
        addArea(area_set);
        reset_getarea_ = false;
      }
    }
    else if(area.task == "set_putBucketArea" && area.type == 0){
      if(reset_putarea_ == true){
          area_set.generateArea(area.seq,AreaManager::AREA_PUT,area.area,area.dock);
          addArea(area_set);
          reset_putarea_ = false;
        }
    }
    else if(area.task == "set_getBucketArea" && area.type == 1){
      get_point_ = area.area[0];
      ROS_INFO("get_point_:::::x::%f,y::%f",get_point_[0],get_point_[1]);
    }
    else if(area.task == "set_putBucketArea" && area.type == 1){
      put_point_ = area.area[0];
      //ROS_INFO("angle:::::%f",angle_);
    }

    ROS_INFO("WorkArea...");
}

void BucketTransport::working_exec(){

//bool flag1 = true;
  ros::Rate rate(10);

  while(ros::ok()){
//ROS_INFO("aaaaaaaaaaaa .........");

    if(task_type_pre_!=task_type_){
      if(task_type_==0){
        ROS_INFO("NO_TASK .........");
      }
      if(task_type_==1){
        ROS_INFO("GET_TASK .........");
      }
      if(task_type_==2){
        ROS_INFO("PUT_TASK .........");
     }
    }
    task_type_pre_ = task_type_;
     //ROS_INFO("working .........");

     switch (task_type_) {
       case NO_TASK:
       break;
       case GET_TASK:
       get_task_exec();
       break;
       case PUT_TASK:
       put_task_exec();
       break;
       default:
       break;
     }
     points_robot_.clear();

     ros::spinOnce();
     rate.sleep();
   }
}

void BucketTransport::get_task_exec(){

  if(emergent_info_ == true){
    boost::unique_lock<boost::mutex> lock(data_update_mutex_);
    get_data_.main_status = GetTaskData::EMERGENCY;
    emergent_info_ = false;
   }
   switch (get_data_.main_status) {

    case GetTaskData::POINT_SEARCH:{
      ROS_INFO(" IN  GetTaskData::POINT_SEARCH");
      tf::Transform robot_pose;
      RobotDriverBase::PoseAckInfo pose_ack;
      switch (get_data_.sub_status) {
        case 0:{
        //   robot_pose.setRotation(tf::Quaternion(tf::Vector3(0,0,1),3.14159/6));
        //   sendPoseAdjReq(robot_pose,0,0,1);
        //   get_data_.sub_status = 1;
        }
        break;
        case 1:{
          if(getPoseAdjAck(pose_ack)){
           robot_pose.setRotation(tf::Quaternion(tf::Vector3(0,0,1),-3.14159/3));
           sendPoseAdjReq(robot_pose,0,0,1);
           get_data_.sub_status = 2;
          }
        }
        break;
        case 2:{
          if(getPoseAdjAck(pose_ack)){
            robot_pose.setRotation(tf::Quaternion(tf::Vector3(0,0,1),3.14159/3));
            sendPoseAdjReq(robot_pose,0,0,1);
            get_data_.sub_status = 3;
          }
        }
        break;
        case 3:{
          if(getPoseAdjAck(pose_ack)){
           get_data_.sub_status = 1;
           robot_pose.setRotation(tf::Quaternion(tf::Vector3(0,0,1),0));
           sendPoseAdjReq(robot_pose,0,0,0);
          }
        }
      }
    }
    break;

    case GetTaskData::EMERGENCY:
    {
      //ROS_INFO(" IN  GetTaskData::EMERGENCY");
      tf::Transform stop;
      stop.setOrigin(tf::Vector3(0,0,0));
      tf::Quaternion rotate;
      rotate.setRotation(tf::Vector3(0,0,1),0);
      stop.setRotation(rotate);
      sendPoseAdjReq(stop);
      char order[5] = {'1','Q','X','2',0x0D};
      robot_driver_ptr_->armSendReset(order,sizeof(order));
      get_data_.lock_area_seq = -1;
      get_data_.main_status = GetTaskData::INIT_GET;
      task_type_ = NO_TASK;
      get_data_.pre_main_status =GetTaskData::INIT_GET;
      get_data_.sub_status = 0;
    }
    break;

    case GetTaskData::INIT_GET:
    {
      /**************************
       *********子任务状态*********
       * 0 - 定位点检查
       * 1 - 等待定位点修正完成
       * 2 - 退出主状态
       **************************/
      tf::Transform robot_pose;
      bool ok = false;
      RobotDriverBase::PoseAckInfo pose_ack;

      ROS_INFO(" IN  GetTaskData::INIT_GET");
      switch (get_data_.sub_status) {
      case 0:
        robot_pose = getGlobalPose();
        if(get_area_[get_data_.lock_area_seq].isRobotInDock(robot_pose)){
          //std::cout<<"inDock is true"<<std::endl;
          get_data_.sub_status = 2;
        }else{
          ok = sendPoseAdjReq(robot_pose);
          if(ok){
            get_data_.sub_status = 2;//1111
          }else{
            ROS_ERROR("sendPoseAdjReq失败");
          }
        }
        break;
      case 1:
        if(getPoseAdjAck(pose_ack)){
          //TODO:判断ACK信息
          /**返回初始化阶段重新检查机器人是否在定位点**/
          get_data_.sub_status = 0;
        }
        break;
      case 2:
        get_data_.pre_main_status = get_data_.main_status;
        get_data_.sub_status = 0;
        get_data_.main_status = GetTaskData::FIND_BUCKET;
        break;
      default:

        break;
      }
    }
      break;

    case GetTaskData::BUCKET_GET:
    {
      //ROS_INFO(" IN  GetTaskData::BUCKET_GET");

      RobotDriverBase::PoseAckInfo pose_ack;
      bool have_bucket = false;

      switch (get_data_.sub_status) {
        case 0:
         if(case_info_==0){
           ROS_INFO("case 0: set precapture points");
           case_info_++;
         }
         //
         while(bucket_points_final_.size()!=0){
           tf::Transform bucket2robot;
           tf::Transform bucket_pose;
           double angle;
           int index;
           GetBucketPose(have_bucket,bucket2robot,angle,bucket_pose,index);
           preCapturePoints(bucket2robot,angle,bucket_pose);
          
           bool ok = false;
           ok = preCaptureSelect(pose_);
           ROS_INFO("PRECAPETURE::%d",ok);
           if(!ok){
             bucket_points_final_.erase(bucket_points_final_.begin()+index);
             continue;
           }
           bucket_pose_=bucket_pose;
           precapture2robot_final_ = global_pose_*pose_;
        //   preCapture_show(bucket_pose_,precapture2robot_final_);
           ROS_INFO("pre::%f,y::%f",precapture2robot_final_.getOrigin().getX(),precapture2robot_final_.getOrigin().getY());
           tf::Vector3 direct = precapture2robot_final_.getOrigin()-global_pose_.getOrigin();
           double direct_angle = direct.angle(tf::Vector3(1,0,0));
           direct_angle = direct.cross(tf::Vector3(1,0,0)).z() > 0 ? -angle : angle;
           tf::Transform straight2pre;
           straight2pre.setRotation(tf::Quaternion(tf::Vector3(0,0,1),angle));
           straight2pre.setOrigin(precapture2robot_final_.getOrigin());
           straight2pre_ = global_pose_.inverse()*straight2pre;

           if(have_bucket){
             sendPoseAdjReq(straight2pre_,1,1,1,0.6,0.6);
             get_data_.sub_status = 1;
           }
           break;
        }


        break;
        case 1:
           if(case_info_==1){
              ROS_INFO("case 1: judge if reach precapture points");
              case_info_++;
           }
           if(getPoseAdjAck(pose_ack)){
              get_data_.sub_status = 2;
           }
           else{
            // get_data_.sub_status = 0;
           }
        break;
        case 2:{
           if(case_info_==2){
             ROS_INFO("case 2: open camera && send rotate");
             case_info_++;
           }
           camera_control_.data = 1;
           camera_control_pub_.publish(camera_control_);
      
           tf::Vector3 direct = global_pose_.getOrigin()-bucket_pose_.getOrigin();
           double angle = direct.angle(tf::Vector3(1,0,0));
           angle = direct.cross(tf::Vector3(1,0,0)).z() > 0 ? -angle : angle;

           tf::Transform robotRotate;
           robotRotate.setRotation(tf::Quaternion(tf::Vector3(0,0,1),angle));
           robotRotate.setOrigin(global_pose_.getOrigin());
           robotRotate = global_pose_.inverse()*robotRotate;

            sendPoseAdjReq(robotRotate,0,0,1);
            get_data_.sub_status=3;
         }
        break;
        case 3:
          if(case_info_==3){
            ROS_INFO("case 3: judge if rotate success");
            case_info_++;
          }
          if(getPoseAdjAck(pose_ack)){
            order_ = RobotDriverBase::PREPARECLOSE;
            bool ok = robot_driver_ptr_->serialCommunication(order_);
            if(!ok){
              break;
            }
            stop_adjust_ = true;
            if(camera_adjust_ == 1){
                get_data_.sub_status=4;
            }
            else{
                get_data_.sub_status=50;
            }

          }
        break;

        case 50:{
          ROS_INFO("case 5050505050");
          tf::Transform send_back;
          send_back.setOrigin(tf::Vector3(-2,0,0));
          send_back.setRotation(tf::Quaternion(tf::Vector3(0,0,1),0));
          sendPoseAdjReq(send_back,1,0,0,0.3,0.3,0.3);
          get_data_.sub_status=7;
        }
        break;

//      case 51:{
//          ROS_INFO("case 5151");
//          order_ = RobotDriverBase::STATUESIGAL;
//          bool ok = robot_driver_ptr_->serialCommunication(order_);
//          if(!ok){
//              break;
//          }
//          data_recv_[0] = robot_driver_ptr_->data_flag_[0];
//          if(data_recv_[0] == '2'){
//              get_data_.sub_status = 8;
//              tf::Transform stop;
//              stop.setOrigin(tf::Vector3(0,0,0));
//              stop.setRotation(tf::Quaternion(tf::Vector3(0,0,1),0));
//              sendPoseAdjReq(stop,1,0,0);
//          }
//      }
      break;
        case 4:{
          //std::cout<<"x:"<<circle_center_point_.position.x<<std::endl;
          //std::cout<<"y:"<<circle_center_point_.position.y<<std::endl;

          ROS_INFO("case 4: send rotate by using camera");
          if(case_info_==4){
            ROS_INFO("case 4: send rotate by using camera");
            case_info_++;
          }


          //std::cout<<"circle_point:::"<<circle_center_point_.position.x<<std::endl;
          if(circle_center_point_.position.x!=0){
            tf::Transform pose;
            float angle = atan((circle_center_point_.position.y-4)/circle_center_point_.position.x);
            if(angle<3&&angle>-3){            
 
              pose.setOrigin(tf::Vector3(0,0,0));
              pose.setRotation(tf::Quaternion(tf::Vector3(0,0,1),angle));
            }
            else{
              pose.setRotation(tf::Quaternion(tf::Vector3(0,0,1),0));
            }
            bool ok = false;
            sendPoseAdjReq(pose,0,0,1);
            get_data_.sub_status = 5;
            nearest_points_.clear();
          }

        }
        break;

        case 5:
        ROS_INFO("case 5: judge if rotate success && check error && send prepare close");
          if(case_info_==5){
            ROS_INFO("case 5: judge if rotate success && check error && send prepare close");
            case_info_++;
          }
          if(getPoseAdjAck(pose_ack)){
            get_data_.sub_status=6;
          }
        break;

        case 6:{
          ROS_INFO("case 6: move distance by using camera");
          if(case_info_==6){
            ROS_INFO("case 6: move distance by using camera");
            case_info_++;
          }
          float move_dis = -0.25;
          if(nearest_point_.position.x!=0){
             nearest_points_.push_back(nearest_point_.position.x);
          }
          if(nearest_points_.size()>=10){
            float average = 0.0;
            for(int i=nearest_points_.size()-5;i<nearest_points_.size();i++){
              average += nearest_points_[i];
              std::cout<<"nearest_point_::"<<nearest_points_[i]<<",,";
            }
            average = average/5;

            if(stop_adjust_==true){
              move_dis = average/100 - 1.1;
            }
            else{
              move_dis = average/100 - camera2bucket_dis_;
            }
            ROS_INFO("average:::%f",average/100);
          }
          std::cout<<std::endl;
            std::cout<<"move_dis:"<<move_dis<<std::endl;
          if(move_dis > 0){

            // float angle = atan(fabs(nearest_point_.position.y-0.04)/nearest_point_.position.x);
             tf::Transform pose;
             pose.setOrigin(tf::Vector3(-move_dis,0,0));
            // pose.setRotation(tf::Quaternion(tf::Vector3(0,0,1),angle));
            bool ok = false;
            ok = sendPoseAdjReq(pose,1,0,0,0.3,0.3);
            get_data_.sub_status = 7;
          }
          if(move_dis > -0.2 && move_dis <= 0.18){
            order_ = RobotDriverBase::SENDCLOSE;
            bool ok = robot_driver_ptr_->serialCommunication(order_);
            if(!ok){
              nearest_points_.clear();
              break;
            }
            get_data_.sub_status = 8;
          }
          if(move_dis<=-0.26){
            nearest_points_.clear();
            get_data_.sub_status = 50;
          }
        }
        break;

        case 7:{
          ROS_INFO("case 7: judge button ack || send arm close");
          nearest_points_.clear();
          if(case_info_==7){
              ROS_INFO("case 7: judge button ack || send arm close");
              case_info_++;
          }
          order_ = RobotDriverBase::STATUESIGAL;
          bool ok = robot_driver_ptr_->serialCommunication(order_);
          if(!ok){
              break;
          }
          data_recv_[0] = robot_driver_ptr_->data_flag_[0];
          if(data_recv_[0] == '2'){
              get_data_.sub_status = 8;
              tf::Transform stop;
              stop.setOrigin(tf::Vector3(0,0,0));
              stop.setRotation(tf::Quaternion(tf::Vector3(0,0,1),0));
              sendPoseAdjReq(stop,1,0,0);
              //sleep(5);
          }
          else{
            if(getPoseAdjAck(pose_ack)){
              if(stop_adjust_ == true){
                get_data_.sub_status = 4;
                stop_adjust_ = false;
              }
              else{
                get_data_.sub_status = 6;
              }     
            }
          }
        }
        break;

        case 8:{
          if(case_info_==8){
            ROS_INFO("case 8: judge arm finish ack");
            case_info_ = 0;
          }
          order_ = RobotDriverBase::STATUESIGAL;
          bool ok = robot_driver_ptr_->serialCommunication(order_);
          if(!ok){
            break;
          }
          data_recv_[0] = robot_driver_ptr_->data_flag_[0];
          if(data_recv_[0] == '3'){
              ROS_INFO("read is ok");

              camera_control_.data = 2;
              camera_control_pub_.publish(camera_control_);
              get_data_.sub_status = 10;
          }
        }
        break;

//        case 9:{
//          if(case_info_==9){
//            ROS_INFO("case 9: judge if catch the bucket");
//            case_info_ = 0;
//          }
//          float dis = nearest_point_.position.x/100-camera2bucket_dis_;
//          if(dis<0.2){
//            camera_control_.data = 2;
//            camera_control_pub_.publish(camera_control_);
//          }
//          else{
//            ROS_INFO("ERROR::not catch the bucket!!!");
//            char order[4] = {'1','R','E',0x0D};
//            robot_driver_ptr_->armSendReset(order,sizeof(order));
//            get_data_.sub_status = 0;
//          }
//        }

        case 10:{
          ROS_INFO("case 10::GET_SUCCESS");
          get_data_.main_status = GetTaskData::GET_SUCCESS;
          get_data_.sub_status = 0;
        }
        break;

        default:
        break;
        }
      }
      break;


      case GetTaskData::FIND_BUCKET:
      {
        ROS_INFO("in FIND_BUCKET");
        tf::Transform robot_pose;
        bool ok = false;
        RobotDriverBase::PoseAckInfo pose_ack;
        switch (get_data_.sub_status) {
          case 0:
          {
            robot_pose = getGlobalPose();
            tf::Quaternion rotate;
            rotate.setRotation(tf::Vector3(0,0,1),0);
            std::vector<int> hot_area_num;
            if(get_area_[get_data_.lock_area_seq].real_y_size_>1&&get_area_[get_data_.lock_area_seq].real_x_size_>1)
            {
              for(int x=0;x<get_area_[get_data_.lock_area_seq].real_x_size_;x++)
              {
                if(x%2 == 0)
                {
                  hot_area_num.push_back(x*get_area_[get_data_.lock_area_seq].real_y_size_);
                  hot_area_num.push_back((1+x)*get_area_[get_data_.lock_area_seq].real_y_size_-1);
                }
                if(x%2 != 0)
                {
                  hot_area_num.push_back((1+x)*get_area_[get_data_.lock_area_seq].real_y_size_-1);
                  hot_area_num.push_back(x*get_area_[get_data_.lock_area_seq].real_y_size_);
                }
              }
            }
            else{
              hot_area_num.push_back(0);
              hot_area_num.push_back(get_area_[get_data_.lock_area_seq].hot_areas_.size()-1);
            }

            get_area_[get_data_.lock_area_seq].hot_area_num_ = hot_area_num;
            get_data_.area2robot = GetTargetPoint(get_area_[get_data_.lock_area_seq],hot_area_num,get_area_[get_data_.lock_area_seq].hot_areas_);
            tf::Transform pose = get_data_.area2robot;
            pose.setOrigin(tf::Vector3(0,0,0));

            double roll,pitch,yaw;
            pose.getBasis().getRPY(roll,pitch,yaw);
            ROS_INFO("rotate::%f",yaw*180/3.1415926);
       
            std::cout<<"case 0::adjust angle"<<std::endl;
            sendPoseAdjReq(pose,0,0,1); //只调整角度

            usleep(20000);
            sendPoseAdjReq(pose,0,0,1); //只调整角度
            get_data_.sub_status=1;
           }
           break;
           case 1:
           {
             if(getPoseAdjAck(pose_ack)){
               get_data_.sub_status = 2;
             }
           }
           break;
           case 2:
           {
             tf::Transform pose = get_data_.area2robot;
             pose.setOrigin(tf::Vector3(get_data_.area2robot.getOrigin().length(),0,0));
             sendPoseAdjReq(pose,1,0,0);//只调整x
             std::cout<<"case 2::adjust xxxx"<<std::endl;
            
             usleep(20000);
             sendPoseAdjReq(pose,1,0,0);//只调整x
             get_data_.sub_status=3;
             
           }
           break;
           case 3:
           {
             if(getPoseAdjAck(pose_ack)){
              // std::cout<<"case 3::biaoji"<<std::endl;
               get_area_[get_data_.lock_area_seq].hot_areas_[get_area_[get_data_.lock_area_seq].hot_area_n_].type = AreaManager::NO_EXIST_TARGET_OBJECT;
               get_data_.sub_status = 0;
               get_data_.main_status = GetTaskData::FIND_BUCKET;
            }else{
              //TODO:添加等待超时操作
            }
          }
          break;
          default:
          break;
          }
        }
        break;

    case GetTaskData::GET_SUCCESS:
  //    ROS_INFO("  IN GetTaskData::GET_SUCCESS");
      robot_driver_ptr_->sendGroupControlAck(ctl_);
      get_data_.lock_area_seq = -1;
      get_data_.main_status = GetTaskData::INIT_GET;
      task_type_ = NO_TASK;
      get_data_.pre_main_status =GetTaskData::INIT_GET;
      get_data_.sub_status = 0;
      break;

    case GetTaskData::AVOID_OBSTACLE:
    {
      ROS_INFO("case GetTaskData::AVOID_OBSTACLE");
      tf::Transform robotStop;
      robotStop.setOrigin(tf::Vector3(0,0,0));
      robotStop.setRotation(tf::Quaternion(tf::Vector3(0,0,1),0));
      sendPoseAdjReq(robotStop,1,0,0);
    }
      break;

    default:
      break;
  }

     int already_check = 0;
     for(int i = 0;i < get_area_[get_data_.lock_area_seq].hot_area_num_.size();i++){
       if(get_area_[get_data_.lock_area_seq].hot_areas_[get_area_[get_data_.lock_area_seq].hot_area_num_[i]].type == AreaManager::NO_EXIST_TARGET_OBJECT){
         already_check++;
       }
     }
     if(already_check == get_area_[get_data_.lock_area_seq].hot_area_num_.size()){
       for(int i = 0;i < get_area_[get_data_.lock_area_seq].hot_area_num_.size();i++){
          get_area_[get_data_.lock_area_seq].hot_areas_[get_area_[get_data_.lock_area_seq].hot_area_num_[i]].type = AreaManager::EXIST_TARGET_OBJECT;
       }
     }

    //************ ==
    if(get_data_.main_status == GetTaskData::INIT_GET||get_data_.main_status == GetTaskData::FIND_BUCKET||get_data_.main_status == GetTaskData::POINT_SEARCH){
      /**条筒预选位置判断**/

      if(point_search_type_ == false){
        for(int i = 0;i < bucket_points_.size();)
        {

          if((bucket_points_[i][0]-p1_[0])*(p3_[0]-p1_[0])>0&&(bucket_points_[i][1]-p1_[1])*(p3_[1]-p1_[1])>0&&
             (bucket_points_[i][0]-p3_[0])*(p1_[0]-p3_[0])>0&&(bucket_points_[i][1]-p3_[1])*(p1_[1]-p3_[1])>0){
            i++;
          }

//         tf::Vector3 bucketinarea =get_area_[get_data_.lock_area_seq].area2world_.inverse()*bucket_points_[i];
//         if(bucketinarea[0]>0&&bucketinarea[0]<(get_area_[get_data_.lock_area_seq].real_x_size_*get_area_[get_data_.lock_area_seq].real_y_slice_)&&
//         bucketinarea[1]>0&&bucketinarea[1]<(get_area_[get_data_.lock_area_seq].real_y_size_*get_area_[get_data_.lock_area_seq].real_x_slice_))
//         {
//           i++;
//          }
          else{
           bucket_points_.erase(bucket_points_.begin()+i);
          }
        }
      }
       else{
         for(int i = 0;i < bucket_points_.size();){
           ROS_INFO("bucket_points_X:%f,y:%f",bucket_points_[i][0],bucket_points_[i][1]);
           if(fabs(get_point_[0]-bucket_points_[i][0])>1.5||fabs(get_point_[1]-bucket_points_[i][1])>1.5){
             bucket_points_.erase(bucket_points_.begin()+i);
           }
           else{i++;}
         }
       }

      if(!bucket_points_.empty()){
        bucket_points_final_ = bucket_points_;
        get_data_.main_status = GetTaskData::BUCKET_GET;
        get_data_.sub_status = 0;
      }
    }

    /**检测小车前方是否有障碍**/
//    if((get_data_.main_status == GetTaskData::BUCKET_GET&&get_data_.sub_status == 0)||get_data_.main_status == GetTaskData::FIND_BUCKET||get_data_.main_status == GetTaskData::AVOID_OBSTACLE){
//     int num_obstacle = 0;
//      for(int i=0;i<points_robot_.size();i++){

//        tf::Vector3 points = global_pose_.inverse()*points_robot_[i];
//        if(points.getX()>laser2baselink_&&points.getX()<avoid_obstacle_distance_&&
//           points.getY()>-avoid_obstacle_width_/2&&points.getY()<avoid_obstacle_width_/2){
//          num_obstacle++;
//        }
//      }
//      if(num_obstacle>10){
//        get_data_.pre_main_status = get_data_.main_status;
//        get_data_.main_status = GetTaskData::AVOID_OBSTACLE;
//      }
//      else{
//        if(get_data_.main_status == GetTaskData::AVOID_OBSTACLE){
//          get_data_.main_status = get_data_.pre_main_status;
//          //get_data_.main_status = GetTaskData::BUCKET_GET;
//        }
//      }
	//    }
}

void BucketTransport::put_task_exec(){
  if(emergent_info_ == true){
    boost::unique_lock<boost::mutex> lock(data_update_mutex_);
    put_data_.main_status = PutTaskData::EMERGENCY;
    emergent_info_ = false;
   }

  switch (put_data_.main_status) {
  case PutTaskData::POINT_PUT:{
    ROS_INFO(" IN  PutTaskData::POINT_PUT");
    RobotDriverBase::PoseAckInfo pose_ack;
    tf::Transform pose;
    switch(put_data_.sub_status){
      case 0:{
        ROS_INFO("case 0:send back dis");
        pose.setRotation(tf::Quaternion(tf::Vector3(0,0,1),0));
//        ROS_INFO("POINT_BACK::%f",point_back_dis_);
        pose.setOrigin(tf::Vector3(point_back_dis_,0,0));
        sendPoseAdjReq(pose,1,0,0,0.2,0.2);
//ROS_INFO("111111111");
        put_data_.sub_status = 1;
      }
      break;
      case 1:{
        ROS_INFO("case 1:arm expend");
        if(getPoseAdjAck(pose_ack)){
          bool ok = false;
          order_ = RobotDriverBase::SENDEXPEND;
          ok = robot_driver_ptr_->serialCommunication(order_);
          if(!ok){
            break;
          }
          put_data_.sub_status = 2;
        }
      }
      break;
      case 2:{
        ROS_INFO("case 2:judge if expend complete");
        bool ok = false;
        order_ = RobotDriverBase::STATUESIGAL;
        ok = robot_driver_ptr_->serialCommunication(order_);
        if(!ok){
            break;
        }
        data_recv_[0] = robot_driver_ptr_->data_flag_[0];
        if(data_recv_[0] == '3'){
            ROS_INFO("read is ok");
            put_data_.sub_status = 3;
        }
      }
      break;

      case 3:{
        ROS_INFO("case 3:send forward dis");
        tf::Quaternion rotate;
        rotate.setRotation(tf::Vector3(0,0,1),0);
        tf::Transform pose_rotate;
        pose_rotate.setRotation(rotate);
        pose_rotate.setOrigin(tf::Vector3(-point_back_dis_,0,0));
        sendPoseAdjReq(pose_rotate,1,0,0);
        put_data_.sub_status = 4;
      }
      break;

      case 4:{
        ROS_INFO("case 4:send STOPWORK");
        if(getPoseAdjAck(pose_ack)){
          bool ok = false;
          order_ = RobotDriverBase::STOPWORK;
          ok = robot_driver_ptr_->serialCommunication(order_);
          if(!ok){
            break;
          }
          put_data_.sub_status = 0;
          put_data_.main_status = PutTaskData::PUT_SUCCESS;
        }
      }
      break;
    }
    
  }
  break;
  case PutTaskData::EMERGENCY:{
    ROS_INFO(" IN  PutTaskData::EMERGENCY");
    tf::Transform stop;
    stop.setOrigin(tf::Vector3(0,0,0));
    tf::Quaternion rotate;
    rotate.setRotation(tf::Vector3(0,0,1),0);
    stop.setRotation(rotate);
    sendPoseAdjReq(stop);
    char order[5] = {'1','Q','X','1',0x0D};
    robot_driver_ptr_->armSendReset(order,sizeof(order));
    put_data_.lock_area_seq = -1;
    put_data_.main_status = PutTaskData::MOVE_POSITION;
    task_type_ = NO_TASK;
    put_data_.sub_status = 0;
  }
  break;
  case PutTaskData::MOVE_POSITION:{
    ROS_INFO(" IN  PutTaskData::MOVE_POSITION");
    RobotDriverBase::PoseAckInfo pose_ack;
    switch (put_data_.sub_status) {
    case 0:{
      tf::Transform area2robot = PutAreaSearch(put_area_[put_data_.lock_area_seq],put_area_[put_data_.lock_area_seq].hot_areas_);
      sendPoseAdjReq(area2robot,1,1,1,0.5,0.5,0.5);
      put_data_.sub_status = 1;
    }
      break;
    case 1:
      if(getPoseAdjAck(pose_ack)){
        put_data_.sub_status = 0;
        put_data_.main_status = PutTaskData::BUCKET_PUT;
        put_area_[put_data_.lock_area_seq].hot_areas_[put_area_[put_data_.lock_area_seq].hot_area_n_].type = AreaManager::EXIST_TARGET_OBJECT;
      }
      break;
    default:
      break;
    }
    }
    break;
  case PutTaskData::BUCKET_PUT :{
    ROS_INFO(" IN  PutTaskData::BUCKET_PUT");
    RobotDriverBase::PoseAckInfo pose_ack;
    switch (put_data_.sub_status) {
    case 0:{
      ROS_INFO(" IN  case 0:::begin rotate");
      tf::Quaternion rotate;
      rotate.setRotation(tf::Vector3(0,0,1),-0.5 * 3.1415926);
      tf::Transform pose_rotate;
      pose_rotate.setRotation(rotate);
      pose_rotate.setOrigin(tf::Vector3(0,0,0));
      tf::Transform pose_rotate_world = put_area_[put_data_.lock_area_seq].area2world_*pose_rotate;
      pose_rotate_world.setOrigin(global_pose_.getOrigin());
      tf::Transform pose_rotate_robot = global_pose_.inverse() * pose_rotate_world;
      pose_rotate_robot.setOrigin(tf::Vector3(0,0,0));
      sendPoseAdjReq(pose_rotate_robot,0,0,1,0.5,0.5,0.5);
      put_data_.sub_status = 1;
    }
    break;

    case 1:{
      ROS_INFO(" IN  case 1::send back");
      if(getPoseAdjAck(pose_ack)){
        tf::Quaternion rotate;
        rotate.setRotation(tf::Vector3(0,0,1),0);
        tf::Transform pose_rotate;
        pose_rotate.setRotation(rotate);
        pose_rotate.setOrigin(tf::Vector3(-bucketPut_distance_,0,0));
        sendPoseAdjReq(pose_rotate,1,0,0,0.5,0.5);
        put_data_.sub_status = 2;
      }
    }
    break;
    case 2:{
      if(getPoseAdjAck(pose_ack)){
        put_data_.sub_status = 3;
      }
    }
    break;
    case 3:{
      ROS_INFO(" IN  case 3::arm expend");
      bool ok = false;
      order_ = RobotDriverBase::SENDEXPEND;
      ok = robot_driver_ptr_->serialCommunication(order_);
      if(!ok){
        break;
      }
      ROS_INFO("read is ok");
      put_data_.sub_status = 4;
    }
    break;

    case 4:{
      ROS_INFO(" IN  case 4::judge arm expend complete");
      bool ok = false;
      order_ = RobotDriverBase::STATUESIGAL;
      ok = robot_driver_ptr_->serialCommunication(order_);
      if(!ok){
          break;
      }
      data_recv_[0] = robot_driver_ptr_->data_flag_[0];
      if(data_recv_[0] == '3'){
          ROS_INFO("read is ok");
          put_data_.sub_status = 5;
      }
    }
    break;

    case 5:{
      ROS_INFO(" IN  case 5::send move forward");
      tf::Quaternion rotate;
      rotate.setRotation(tf::Vector3(0,0,1),0);
      tf::Transform pose_rotate;
      pose_rotate.setRotation(rotate);
      pose_rotate.setOrigin(tf::Vector3(bucketPut_distance_,0,0));
      sendPoseAdjReq(pose_rotate,1,0,0);
      put_data_.sub_status = 6;
    }
    break;

    case 6:{
      ROS_INFO(" IN  case 6::send stop work");
      if(getPoseAdjAck(pose_ack)){
        bool ok = false;
        order_ = RobotDriverBase::STOPWORK;
        ok = robot_driver_ptr_->serialCommunication(order_);
        if(!ok){
           break;
        }
        put_data_.sub_status = 0;
        put_data_.main_status = PutTaskData::PUT_SUCCESS;
      }
    }
    break;

    default:
    break;
    }
  }
  break;

  case PutTaskData::PUT_SUCCESS :{
    ROS_INFO(" IN  PutTaskData::PUT_SUCCESS");
    robot_driver_ptr_->sendGroupControlAck(ctl_);
    put_data_.lock_area_seq = -1;
    put_data_.main_status = PutTaskData::MOVE_POSITION;
    task_type_ = NO_TASK;
    put_data_.sub_status = 0;
bucket_points_.clear();
  }
  break;

  case PutTaskData::AVOID_OBSTACLE_PUT :
  {
    ROS_INFO("PutTaskData::AVOID_OBSTACLE_PUT");
    tf::Transform robotStop;
    robotStop.setOrigin(tf::Vector3(0,0,0));
    robotStop.setRotation(tf::Quaternion(tf::Vector3(0,0,1),0));
    sendPoseAdjReq(robotStop,1,0,0);
  }
    break;
    default:
    break;
  }

  /**检测小车前方是否有障碍**/
//  if(put_data_.main_status == PutTaskData::MOVE_POSITION||put_data_.main_status == PutTaskData::AVOID_OBSTACLE_PUT){
//    int num_obstacle = 0;
//    for(int i=0;i<points_robot_.size();i++){
//      tf::Vector3 points = global_pose_.inverse()*points_robot_[i];
//      if(points.getX()>0&&points.getX()<avoid_obstacle_distance_&&
//         points.getY()>-avoid_obstacle_width_/2&&points.getY()<avoid_obstacle_width_/2){
//        num_obstacle++;
//      }
//    }
//    if(num_obstacle>10){
//      put_data_.main_status = PutTaskData::AVOID_OBSTACLE_PUT;
//    }
//    else{
//      if(put_data_.main_status == PutTaskData::AVOID_OBSTACLE_PUT){
//        put_data_.main_status = PutTaskData::MOVE_POSITION;
//      }
//    }
//  }

//   /**检测放桶区域内是否存在障碍物**/
//   AreaManager::HotArea hot_area_now = put_area_[put_data_.lock_area_seq].hot_areas_[put_area_[put_data_.lock_area_seq].hot_area_n_];
//   tf::Vector3 hot_point_world = put_area_[put_data_.lock_area_seq].area2world_ * hot_area_now.hot_point;
//   bool obstacle_exist = ObstacleInHotArea(hot_point_world);
//   if(obstacle_exist){
//     put_area_[put_data_.lock_area_seq].hot_areas_[put_area_[put_data_.lock_area_seq].hot_area_n_].type = AreaManager::EXIST_TARGET_OBJECT;
//   }
}

bool BucketTransport::addArea(const AreaManager& area){
  boost::unique_lock<boost::mutex> lock(data_update_mutex_);
  if(area.area_type_ == AreaManager::AREA_GET){
    if(isLockGetArea(area.area_seq_)){
      return false;
    }else{
      get_area_[area.area_seq_] = area;
      return true;
    }
  }else if(area.area_type_ == AreaManager::AREA_PUT){
    if(isLockPutArea(area.area_seq_)){
      return false;
    }else{
      put_area_[area.area_seq_] = area;
      return true;
    }
  }else{
    return false;
  }
  
}

BucketTransport::TaskReqStatus BucketTransport::requestGetTask(RobotDriverBase::GroupControlInfo ctl,int index){
  boost::unique_lock<boost::mutex> lock(data_update_mutex_);

  if(task_type_ != NO_TASK){
    return BucketTransport::TASK_EXECUTING;
  }

  if(index == -1){
    std::map<int,AreaManager>::iterator it;
    for(it = get_area_.begin();it != get_area_.end();it++){
      if(it->second.isCloseBorder(global_pose_)){
        index = it->first;
        break;
      }
    }
    if(index != -1){
      task_type_ = GET_TASK;
      //get_data_.ctl_ = ctl;
      get_data_.lock_area_seq = index; /*锁定任务区域*/
      return BucketTransport::TASK_REQ_SUCCESS;
    }else{
      return BucketTransport::AREA_NO_EXIST;
    }
  }else{
    if(get_area_.find(index) != get_area_.end()){;
      if(get_area_[index].isCloseBorder(global_pose_)){
        task_type_ = GET_TASK;
        //get_data_.ctl_ = ctl;
        get_data_.lock_area_seq = index; /*锁定任务区域*/
        return BucketTransport::TASK_REQ_SUCCESS;
      }else{
        return BucketTransport::AREA_NO_EXIST;
      }
    }else{
      return BucketTransport::AREA_NO_EXIST;
    }
  }
}

BucketTransport::TaskReqStatus BucketTransport::requestPutTask(RobotDriverBase::GroupControlInfo ctl,int index){
  boost::unique_lock<boost::mutex> lock(data_update_mutex_);

  if(task_type_ != NO_TASK){
    return BucketTransport::TASK_EXECUTING;
  }

  if(index == -1){
    std::map<int,AreaManager>::iterator it;
    for(it = put_area_.begin();it != put_area_.end();it++){
      if(it->second.isCloseBorder(global_pose_)){
        index = it->first;
        break;
      }
    }
    if(index != -1){
      task_type_ = PUT_TASK;
      //put_data_.ctl_ = ctl;
      put_data_.lock_area_seq = index; /*锁定任务区域*/
      return BucketTransport::TASK_REQ_SUCCESS;
    }else{
      return BucketTransport::AREA_NO_EXIST;
    }
  }else{
    if(put_area_.find(index) != put_area_.end()){
      if(put_area_[index].isCloseBorder(global_pose_)){
        task_type_ = PUT_TASK;
        //put_data_.ctl_ = ctl;
        put_data_.lock_area_seq = index; /*锁定任务区域*/
        return BucketTransport::TASK_REQ_SUCCESS;
      }else{
        return BucketTransport::AREA_NO_EXIST;
      }
    }else{
      return BucketTransport::AREA_NO_EXIST;
    }
  }
}

bool BucketTransport::isLockGetArea(int index){
  return get_data_.lock_area_seq == index ? 1 : 0;
}

bool BucketTransport::isLockPutArea(int index){
  return put_data_.lock_area_seq == index ? 1 : 0;
}

tf::Transform BucketTransport::getGlobalPose(){
  boost::unique_lock<boost::mutex> lock(data_update_mutex_);
  return global_pose_;
}

std::vector<tf::Vector3> BucketTransport::getBucketPosition(){
  boost::unique_lock<boost::mutex> lock(data_update_mutex_);
  return bucket_points_;
}

bool BucketTransport::sendPoseAdjReq(const tf::Transform& pose,bool sw_x,bool sw_y,bool sw_th){
  bool ok = false;
  double roll,pitch,yaw;
  pose.getBasis().getRPY(roll,pitch,yaw);

  ok = robot_driver_ptr_->sendPoseAdjustRequest(2,sw_x,sw_y,sw_th,pose.getOrigin().x(),
                                           pose.getOrigin().y(),
                                           yaw,
                                           max_vel_x_,max_vel_y_,max_vel_th_,
                                           precision_x_,precision_y_,precision_th_);

  if(ok){
    boost::unique_lock<boost::mutex> lock(data_update_mutex_);
    recv_pose_ack_ = false;
    return true;
  }else{
    return false;
  }
}

bool BucketTransport::sendPoseAdjReq(const tf::Transform& pose,bool sw_x,bool sw_y,bool sw_th,double max_vel_x, double max_vel_y){
  bool ok = false;
  double roll,pitch,yaw;
  pose.getBasis().getRPY(roll,pitch,yaw);
  ok = robot_driver_ptr_->sendPoseAdjustRequest(2,sw_x,sw_y,sw_th,pose.getOrigin().x(),
                                           pose.getOrigin().y(),
                                           yaw,
                                           max_vel_x,max_vel_y,max_vel_th_,
                                           precision_x_,precision_y_,precision_th_);
  if(ok){
    boost::unique_lock<boost::mutex> lock(data_update_mutex_);
    recv_pose_ack_ = false;
    return true;
  }else{
    return false;
  }
}

bool BucketTransport::sendPoseAdjReq(const tf::Transform& pose,bool sw_x,bool sw_y,bool sw_th,double max_vel_x, double max_vel_y, double max_vel_th){
  bool ok = false;
  double roll,pitch,yaw;
  pose.getBasis().getRPY(roll,pitch,yaw);
  ok = robot_driver_ptr_->sendPoseAdjustRequest(2,sw_x,sw_y,sw_th,pose.getOrigin().x(),
                                           pose.getOrigin().y(),
                                           yaw,
                                           max_vel_x,max_vel_y,max_vel_th,
                                           precision_x_,precision_y_,precision_th_);
  if(ok){
    boost::unique_lock<boost::mutex> lock(data_update_mutex_);
    recv_pose_ack_ = false;
    return true;
  }else{
    return false;
  }
}

bool BucketTransport::getPoseAdjAck(RobotDriverBase::PoseAckInfo& ack){
  boost::unique_lock<boost::mutex> lock(data_update_mutex_);
 // std::cout<<"jin ru ack..........."<<recv_pose_ack_<<std::endl;
  if(recv_pose_ack_){
    ack = pose_ack_;
    recv_pose_ack_ = false;
    return true;
  }else{
    return false;
  }
}

tf::Transform BucketTransport::GetBucketPose(bool& have_bucket,tf::Transform& bucket2robot,double& angle,tf::Transform& bucket_pose,int &index){
  if(bucket_points_.size()==0)
  {
    tf::Transform bucket2robot;
    bucket2robot.setOrigin(tf::Vector3(0,0,0));
    tf::Quaternion rotate;
    rotate.setRotation(tf::Vector3(0,0,1),0);
    bucket2robot.setRotation(rotate);
    have_bucket = false;
    return bucket2robot;
  }
  else{
    boost::unique_lock<boost::mutex> lock(data_update_mutex_);
    std::vector<tf::Vector3>::iterator it = bucket_points_.begin();
    tf::Vector3 min = *it;

    for(it=bucket_points_.begin();it != bucket_points_.end();it++){
      tf::Vector3 robot = global_pose_.getOrigin();
      if((robot - min).length() > (robot - *it).length()){
        min = *it;
      }
    }

    for(int i=0;i<bucket_points_.size();i++){
      if(min == bucket_points_[i]){
        index = i;
        break;
      }
    }
    bucket_pose.setIdentity();
    bucket_pose.setOrigin(min);
//    ROS_INFO("MIN::%f,,%f",min[0],min[1]);
    min = min - global_pose_.getOrigin();
    angle = min.angle(tf::Vector3(1,0,0));
    angle = min.cross(tf::Vector3(1,0,0)).z() > 0 ? -angle : angle;  //与世界坐标系的x轴夹角
    bucket_pose.setRotation(tf::Quaternion(tf::Vector3(0,0,1),angle));// * 3.1415926 / 180.0
    bucket2robot = global_pose_.inverse() * bucket_pose;
    double rate = (bucket2robot.getOrigin().length() - distance_bucket_) / bucket2robot.getOrigin().length();
    bucket2robot.setOrigin(bucket2robot.getOrigin() * rate);
    have_bucket = true;
    return bucket2robot;
  }
}

void BucketTransport::preCapturePoints(tf::Transform bucket2robot,double angle,tf::Transform bucket_pose){
  bucket_precapture_points_.clear();
  //第一个预捕获点的世界坐标
  tf::Transform bucket_precapture_one;
  bucket_precapture_one = global_pose_*bucket2robot;
  bucket_precapture_points_.push_back(bucket_precapture_one);
  //第二个预捕获点的世界坐标
  float second_angle = (3.14159-angle-precapture_angle_);
  tf::Transform bucket_precapture_two;
  tf::Vector3 pointTwo_world;
  pointTwo_world[0] = bucket_pose.getOrigin().getX() + distance_bucket_*cos(second_angle);
  pointTwo_world[1] = bucket_pose.getOrigin().getY() - distance_bucket_*sin(second_angle);
  pointTwo_world[2] = 0;
  bucket_precapture_two.setRotation(tf::Quaternion(tf::Vector3(0,0,1),angle+precapture_angle_));
  bucket_precapture_two.setOrigin(pointTwo_world);
  bucket_precapture_points_.push_back(bucket_precapture_two);
  //第三个预捕获点的世界坐标
  float third_angle = 3.14159-angle+precapture_angle_;
  tf::Transform bucket_precapture_three;
  tf::Vector3 pointThree_world;
  pointThree_world[0] = bucket_pose.getOrigin().getX() + distance_bucket_*cos(third_angle);
  pointThree_world[1] = bucket_pose.getOrigin().getY() - distance_bucket_*sin(third_angle);
  pointThree_world[2] = 0;
  bucket_precapture_three.setRotation(tf::Quaternion(tf::Vector3(0,0,1),angle-precapture_angle_));
  bucket_precapture_three.setOrigin(pointThree_world);
  bucket_precapture_points_.push_back(bucket_precapture_three);
  //第四个预捕获点的世界坐标
  float fourth_angle = 3.14159-angle-2*precapture_angle_;
  tf::Transform bucket_precapture_four;
  tf::Vector3 pointFour_world;
  pointFour_world[0] = bucket_pose.getOrigin().getX() + distance_bucket_*cos(fourth_angle);
  pointFour_world[1] = bucket_pose.getOrigin().getY() - distance_bucket_*sin(fourth_angle);
  pointFour_world[2] = 0;
  bucket_precapture_four.setRotation(tf::Quaternion(tf::Vector3(0,0,1),angle+2*precapture_angle_));
  bucket_precapture_four.setOrigin(pointFour_world);
  bucket_precapture_points_.push_back(bucket_precapture_four);
  //第五个预捕获点的世界坐标
  float fifth_angle = 3.14159-angle+2*precapture_angle_;
  tf::Transform bucket_precapture_five;
  tf::Vector3 pointFifth_world;
  pointFifth_world[0] = bucket_pose.getOrigin().getX() + distance_bucket_*cos(fifth_angle);
  pointFifth_world[1] = bucket_pose.getOrigin().getY() - distance_bucket_*sin(fifth_angle);
  pointFifth_world[2] = 0;
  bucket_precapture_five.setRotation(tf::Quaternion(tf::Vector3(0,0,1),angle-2*precapture_angle_));
  bucket_precapture_five.setOrigin(pointFifth_world);
  bucket_precapture_points_.push_back(bucket_precapture_five);
}

bool BucketTransport::preCaptureSelect(tf::Transform &pose){
  for(int i=0;i<bucket_precapture_points_.size();){
    tf::Transform precapture2robot = global_pose_.inverse()*bucket_precapture_points_[i];
    tf::Vector3 precapture_robot = precapture2robot.getOrigin();
    int obstacle_num = 0;
    for(int j=0;j<points_robot_.size();j++){
      tf::Vector3 points_rob = global_pose_.inverse()*points_robot_[j];
      if(points_rob.getX()>laser2baselink_){
        if((points_rob.getX()-precapture_robot.getX())*(points_rob.getX()-precapture_robot.getX())+
           (points_rob.getY()-precapture_robot.getY())*(points_rob.getY()-precapture_robot.getY())<precapture_radius_*precapture_radius_){
          obstacle_num++;
        }
      }
    }
    if(obstacle_num<10){
      pose = precapture2robot;
      return true;
    }
    else{
      i++;
    }
  }
  return false;
}

void BucketTransport::preCapture_show(tf::Transform bucket_pose,tf::Transform& precapture2robot_final){
  ROS_INFO("11");
  cv::Mat result(1200,960,CV_32FC3,cv::Scalar(0,0,0));
  ROS_INFO("BUCK::%f,%f",bucket_pose.getOrigin().getX(),bucket_pose.getOrigin().getY());
  result.at<cv::Vec3f>(bucket_pose.getOrigin().getX()*100,bucket_pose.getOrigin().getY()*100+400)[0]=255;
  result.at<cv::Vec3f>(bucket_pose.getOrigin().getX()*100,bucket_pose.getOrigin().getY()*100+400)[1]=255;
  result.at<cv::Vec3f>(bucket_pose.getOrigin().getX()*100,bucket_pose.getOrigin().getY()*100+400)[2]=255;
  ROS_INFO("AA");
  result.at<cv::Vec3f>(global_pose_.getOrigin().getX()*100,global_pose_.getOrigin().getY()*100+400)[0]=255;
  result.at<cv::Vec3f>(global_pose_.getOrigin().getX()*100,global_pose_.getOrigin().getY()*100+400)[1]=255;
  result.at<cv::Vec3f>(global_pose_.getOrigin().getX()*100,global_pose_.getOrigin().getY()*100+400)[2]=0;
  ROS_INFO("BB");
  for(int i=0;i<bucket_precapture_points_.size();i++){
    if(bucket_precapture_points_[i].getOrigin().getX()*100>0){
      result.at<cv::Vec3f>(bucket_precapture_points_[i].getOrigin().getX()*100,bucket_precapture_points_[i].getOrigin().getY()*100+400)[0]=0;
      result.at<cv::Vec3f>(bucket_precapture_points_[i].getOrigin().getX()*100,bucket_precapture_points_[i].getOrigin().getY()*100+400)[1]=255;
      result.at<cv::Vec3f>(bucket_precapture_points_[i].getOrigin().getX()*100,bucket_precapture_points_[i].getOrigin().getY()*100+400)[2]=255;
      cv::circle(result,cv::Point2f(bucket_precapture_points_[i].getOrigin().getY()*100+400,bucket_precapture_points_[i].getOrigin().getX()*100),precapture_radius_*100,3);

    }
  }
  ROS_INFO("222");
  if(precapture2robot_final.getOrigin().getX()*100>0){
    cv::rectangle(result,cv::Rect(precapture2robot_final.getOrigin().getY()*100+400-10,precapture2robot_final.getOrigin().getX()*100-10,20,20),cv::Scalar(0,255,0),1);
  }
  cv::rectangle(result,cv::Rect(global_pose_.getOrigin().getY()*100+400-20,global_pose_.getOrigin().getX()*100-40,40,80),cv::Scalar(255,255,0),1);
  cv::circle(result,cv::Point2f(bucket_pose.getOrigin().getY()*100+400,bucket_pose.getOrigin().getX()*100),target_radius_*100,cv::Scalar(255,255,255),1);
  for(int i=0;i<points_robot_.size();i++){
    tf::Vector3 laser_world = points_robot_[i];
    if(laser_world[0]*100>0&&laser_world[0]*100<1000&&laser_world[1]*100+400>0&&laser_world[1]*100+400<900){
      result.at<cv::Vec3f>(laser_world[0]*100,laser_world[1]*100+400)[0]=255;
      result.at<cv::Vec3f>(laser_world[0]*100,laser_world[1]*100+400)[1]=255;
      result.at<cv::Vec3f>(laser_world[0]*100,laser_world[1]*100+400)[2]=255;
    }
  }
  ROS_INFO("333");
  cv::imshow("result",result);
  cv::waitKey(10);
  ROS_INFO("444");
}

tf::Transform BucketTransport::GetTargetPoint(AreaManager& area, std::vector<int> hot_area_num,std::vector<AreaManager::HotArea> hot_areas){
for(int i =0;i<hot_area_num.size();i++){
std::cout<<"point.x:"<<hot_areas[hot_area_num[i]].hot_point[0]<<"point.y:"<<hot_areas[hot_area_num[i]].hot_point[1]<<std::endl;
}
  for(int i=0;i<hot_area_num.size();i++){
    if(hot_areas[hot_area_num[i]].type != AreaManager::NO_EXIST_TARGET_OBJECT){
      ROS_INFO("point_num:::%d",i);
       area.hot_area_n_ = hot_area_num[i];
       tf::Vector3 hot_area_origen = hot_areas[hot_area_num[i]].hot_point;
       tf::Vector3 hot_area_world = area.area2world_*hot_area_origen;

       std::cout<<"area.area2world_::"<<area.area2world_.getOrigin().x()<<",y::"<<area.area2world_.getOrigin().y()<<std::endl;
       tf::Transform bucket_pose;
       bucket_pose.setIdentity();
       bucket_pose.setOrigin(hot_area_world);
       hot_area_world = hot_area_world - global_pose_.getOrigin();
       double angle = hot_area_world.angle(tf::Vector3(1,0,0));
       angle = hot_area_world.cross(tf::Vector3(1,0,0)).z() > 0 ? -angle : angle;
       bucket_pose.setRotation(tf::Quaternion(tf::Vector3(0,0,1),angle));
//       ROS_INFO("angle:::%f",angle);
       tf::Transform areapoints2robot = global_pose_.inverse() * bucket_pose;
//       double roll,pitch,yaw;
//       areapoints2robot.getBasis().getRPY(roll,pitch,yaw);
//       ROS_INFO("areapoint2robot::%f,%f,%f,%f",areapoints2robot.getOrigin().x(),areapoints2robot.getOrigin().y(),areapoints2robot.getOrigin().z(),yaw*180/3.1415926);
//       if(i!=0&&i!=1){
//        if(i%4==2||i%4==3){
//          areapoints2robot.setRotation(tf::Quaternion(tf::Vector3(0,0,1),-3.14159));
//         }
//        if(i%4==0||i%4==1){
//          areapoints2robot.setRotation(tf::Quaternion(tf::Vector3(0,0,1),3.14159));
//        }
//        if(i==1){
//          areapoints2robot.setRotation(tf::Quaternion(tf::Vector3(0,0,1),0));
//        }
//       }
//     if(i==1){
//areapoints2robot.setRotation(tf::Quaternion(tf::Vector3(0,0,1),0));
//}
       return areapoints2robot;
    }
  }
}

tf::Transform BucketTransport::PutAreaSearch(AreaManager& area,std::vector<AreaManager::HotArea> hot_areas){
  for(int i=hot_areas.size()-1;i>=0;i--){
    if(hot_areas[i].type == AreaManager::NO_EXIST_TARGET_OBJECT){
       area.hot_area_n_ = i;
       tf::Vector3 hot_area_origen = hot_areas[i].hot_point;
       hot_area_origen[1] = hot_area_origen[1] - put_data_.robot_bucket_dis;
       tf::Vector3 hot_area_world = area.area2world_*hot_area_origen;
       tf::Transform bucket_pose;
       bucket_pose.setIdentity();
       bucket_pose.setOrigin(hot_area_world);
       hot_area_world = hot_area_world - global_pose_.getOrigin();
       double angle = hot_area_world.angle(tf::Vector3(1,0,0));
       angle = hot_area_world.cross(tf::Vector3(1,0,0)).z() > 0 ? -angle : angle;
       bucket_pose.setRotation(tf::Quaternion(tf::Vector3(0,0,1),angle));
       ROS_INFO("PUT :destination::%f,%f,%f",bucket_pose.getOrigin().x(),bucket_pose.getOrigin().y(),bucket_pose.getOrigin().z());

       tf::Transform areapoints2robot = global_pose_.inverse() * bucket_pose;
       double roll,pitch,yaw;
       areapoints2robot.getBasis().getRPY(roll,pitch,yaw);
       ROS_INFO("PUT :areapoint2robot::%f,%f,%f,%f",areapoints2robot.getOrigin().x(),areapoints2robot.getOrigin().y(),areapoints2robot.getOrigin().z(),yaw*180/3.1415926);
       return areapoints2robot;
    }
  }
}

AreaManager::AreaManager(float hot_area_len, float target_object_radius, float distance_of_expandsion):
  hot_area_len_(hot_area_len),
  target_object_radius_(target_object_radius),
  distance_of_expandsion_(distance_of_expandsion){
}

bool AreaManager::isCloseBorder(const tf::Transform &pose){
  tf::Vector3 origin =AreaManager::area2world_.inverse() * pose.getOrigin();
  
  double x_max = real_x_size_ * real_x_slice_ + distance_of_expandsion_;
  double x_min = -distance_of_expandsion_;

  double y_max = real_y_size_ * real_y_slice_ + distance_of_expandsion_;
  double y_min = -distance_of_expandsion_;
  if(origin.x() >= x_min && origin.x() <= x_max && origin.y() >= y_min && origin.y() <= y_max){
    return true;
  }else{
    return false;
  }
}

tf::Transform AreaManager::getDockGlobalPose(){
  tf::Transform pose;
  tf::Quaternion rotate;
  tf::Vector3 dock_area = area2world_.inverse() * dock_;

  rotate.setRotation(tf::Vector3(0,0,1),0.5 * 3.1415926);
  pose.setRotation(rotate);
  pose.setOrigin(dock_area);

  pose = area2world_ * pose;

  return pose;
}

bool AreaManager::isRobotInDock(tf::Transform &pose){
  tf::Transform dock2area;
  tf::Quaternion rotate;
  rotate.setRotation(tf::Vector3(0,0,1),0.5 * 3.1415926);
  dock2area.setRotation(rotate);
  dock2area.setOrigin(area2world_.inverse() * dock_);
  double roll,pitch,yaw;
  dock2area.getBasis().getRPY(roll,pitch,yaw);
//ROS_INFO("dock2area::%f,%f,%f,%f",dock2area.getOrigin().x(),dock2area.getOrigin().y(),dock2area.getOrigin().z(),
 //      yaw * 180 / 3.1415926);

  tf::Transform robot2area = area2world_.inverse() * pose;

  robot2area.getBasis().getRPY(roll,pitch,yaw);
 // ROS_INFO("robot2area::%f,%f,%f,%f",robot2area.getOrigin().x(),robot2area.getOrigin().y(),robot2area.getOrigin().z(),
//  yaw * 180 / 3.1415926);

  tf::Transform dock2robot = robot2area.inverse() * dock2area;

  dock2robot.getBasis().getRPY(roll,pitch,yaw);
 // ROS_INFO("dock2robot::%f,%f,%f,%f",dock2robot.getOrigin().x(),dock2robot.getOrigin().y(),dock2robot.getOrigin().z(),
 // yaw * 180 / 3.1415926);
  tf::Vector3 origin = robot2area.getOrigin() - dock2area.getOrigin();
 // std::cout<<"y:"<<origin.y()<<std::endl;
  /**机器人位置偏移低于0.3m**/
  if(fabs(origin.y()) > 0.3){
    pose = dock2robot;
    return false;
  }

  double angle = fabs(dock2robot.getRotation().getAngle()) * 180.0 / 3.1415926;
//std::cout<<"angel:"<<angle<<std::endl;
  /**机器人角度偏移低于10度**/
  if(angle > 10.0){
    pose = dock2robot;
    return false;
  }
  return true;
}

tf::Transform AreaManager::getPositionGlobalPose(HotArea hot_area){
  tf::Transform pose;
  tf::Quaternion rotate;
  rotate.setRotation(tf::Vector3(0,0,1),1 * 3.1415926);
  pose.setRotation(rotate);
  pose.setOrigin(hot_area.hot_point);

  pose = area2world_ * pose;

  return pose;
}

bool AreaManager::isRobotInPosition(HotArea hot_area,tf::Transform &pose){
  tf::Transform position2map = getPositionGlobalPose(hot_area);

  tf::Transform robot2position = position2map.inverse() * pose;
  tf::Transform position2robot = robot2position.inverse();
  tf::Vector3 origin = position2robot .getOrigin();
  /**机器人位置偏移低于0.3m**/
  if(origin.length() > 0.3){
    pose = position2robot;
    return false;
  }

  double angle = fabs(position2robot.getRotation().getAngle()) * 180.0 / 3.1415926;

  /**机器人角度偏移低于10度**/
  if(angle > 10.0){
    pose = position2robot;
    return false;
  }
  return true;
}

int AreaManager::checkBucketPosition( std::vector<tf::Vector3>& bucket_points){
  std::vector<tf::Vector3>::iterator it;
  for(it = bucket_points.begin();it != bucket_points.end();){
    tf::Vector3 bucket_point = area2world_.inverse() * (*it);
    if(bucket_point.x() < 0 || bucket_point.x() > real_x_slice_ * real_x_size_){

    }
  }
}

bool AreaManager::BucketInHotArea(HotArea& hot_area,std::vector<tf::Vector3> bucket_points){
  std::vector<tf::Vector3>::iterator it;
  for(it=bucket_points.begin();it != bucket_points.end();it++){
    tf::Vector3 bucket_point = area2world_.inverse() * (*it);
    float dis = sqrt((hot_area.hot_point.getX()-bucket_point.getX())*(hot_area.hot_point.getX()-bucket_point.getX())+
        (hot_area.hot_point.getY()-bucket_point.getY())*(hot_area.hot_point.getY()-bucket_point.getY()));
    if(dis<0.4)
    {
      hot_area.type = EXIST_TARGET_OBJECT;
      return true;
    }
  }
  return false;
}

bool BucketTransport::ObstacleInHotArea(tf::Vector3 hot_point_world){
  int obstacle_num = 0;
  std::vector<tf::Vector3>::iterator it;
  for(it=points_robot_.begin();it<points_robot_.end();it++){
     if((*points_robot_[0]-hot_point_world[0])*(*points_robot_[0]-hot_point_world[0])+
        (*points_robot_[0]-hot_point_world[0])*(*points_robot_[0]-hot_point_world[0])<bucket_radius_*bucket_radius_){
       obstacle_num++;
     }
     if(obstacle_num>10){
       return true;
     }
  }
  return false;
}

bool AreaManager::generateArea(int area_seq, AreaType area_type,
                               const std::vector<tf::Vector3>& points,
                               const tf::Vector3 dock){

  if(points.size() != 4){
    ROS_ERROR("区域初始化失败，请提供矩形区域的4个顶点坐标");
    return false;
  }

  area_seq_ = area_seq;
  area_type_ = area_type;
  area_points_ = points;
  dock_ = dock;
  tf::Vector3 x_axis = area_points_[1] - area_points_[0];
  double angle = x_axis.angle(tf::Vector3(1,0,0));
  tf::Vector3 z_x = x_axis.cross(tf::Vector3(1,0,0));
  angle = z_x.z() >= 0 ? -angle : angle;

  tf::Quaternion q;
  q.setRotation(tf::Vector3(0,0,1),angle);
  tf::Transform area2world;
  area2world.setIdentity();
  area2world.setOrigin(area_points_[0]);
  area2world.setRotation(q);
  area2world_ = area2world;
  tf::Vector3 y_a = area2world.getBasis().getColumn(1);

  double x_len = x_axis.length();
  tf::Vector3 y_axis = area_points_[3] - area_points_[0];
  double y_len = y_axis.dot(y_a);
  y_len = fabs(y_len);

  if(area_type_ == AREA_GET){
    if(x_len < hot_area_len_ / 2.0 || y_len < hot_area_len_ / 2.0){
      ROS_INFO("区域长宽设置过小，长宽应大于%f米",hot_area_len_/2.0);
      return false;
    }



    real_x_size_ = round(x_len / hot_area_len_);
    real_x_slice_ = x_len / real_x_size_;
    real_y_size_ = round(y_len / hot_area_len_);
    real_y_slice_ = y_len / real_y_size_;
    hot_areas_.resize(real_x_size_ * real_y_size_);
  }

  if(area_type_ == AREA_PUT){
    if(x_len < target_object_radius_ * 3 || y_len < target_object_radius_ * 3){
      ROS_INFO("区域长宽设置过小，长宽应大于%f米",target_object_radius_ * 3);
      return false;
    }

    real_x_size_ = int(x_len / (target_object_radius_ * 2));
    real_x_slice_ = x_len / real_x_size_;
    real_y_size_ = int(y_len / (target_object_radius_ * 2));
    real_y_slice_ = y_len / real_y_size_;
    hot_areas_.resize(real_x_size_ * real_y_size_);
  }

  
  for(int i=0;i<real_x_size_;i++){
    for(int j=0;j<real_y_size_;j++){
      HotArea area;
      if(area_type_ == AREA_GET){
        area.type = EXIST_TARGET_OBJECT;
      }
      else if(area_type_ == AREA_PUT){
        area.type = NO_EXIST_TARGET_OBJECT;
      }
      area.hot_point.setZero();
      area.hot_point.setX(i * real_x_slice_ + real_x_slice_ / 2.0);
      area.hot_point.setY(j * real_y_slice_ + real_y_slice_ / 2.0);
      hot_areas_[i * real_y_size_ + j] = area;
    }
  }
  
}

void AreaManager::areaVisualization(AreaGrid& area_visual){

  //创建一个 visualization_msgs/Marker消息，并且初始化所有共享的数据。消息成员默认为0，仅仅设置位姿成员w。
  area_visual.points.header.frame_id = area_visual.line_strip.header.frame_id = area_visual.line_list.header.frame_id = "/map";
  area_visual.points.header.stamp = area_visual.line_strip.header.stamp = area_visual.line_list.header.stamp = ros::Time::now();
  area_visual.points.ns = area_visual.line_strip.ns = area_visual.line_list.ns = "points_and_lines";
  area_visual.points.action = area_visual.line_strip.action = area_visual.line_list.action = visualization_msgs::Marker::ADD;
  area_visual.points.pose.orientation.w = area_visual.line_strip.pose.orientation.w = area_visual. line_list.pose.orientation.w = 1.0;

  //分配三个不同的id到三个markers。points_and_lines名称空间的使用确保彼此不会相互冲突。
  area_visual.points.id = 0;
  area_visual.line_strip.id = 1;
  area_visual.line_list.id = 2;

  //设置marker类型到 POINTS, LINE_STRIP 和 LINE_LIST
  area_visual.points.type = visualization_msgs::Marker::POINTS;
  area_visual.line_strip.type = visualization_msgs::Marker::LINE_STRIP;
  area_visual.line_list.type = visualization_msgs::Marker::LINE_LIST;

  // scale成员对于这些marker类型是不同的,POINTS marker分别使用x和y作为宽和高，然而LINE_STRIP和LINE_LIST marker仅仅使用x，定义为线的宽度。单位是米。
  area_visual.points.scale.x = 0.2;
  area_visual.points.scale.y = 0.2;

  // LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width
  area_visual.line_strip.scale.x = 0.1;
  area_visual.line_list.scale.x = 0.1;

  // 点为绿色
  area_visual.points.color.g = 1.0f;
  area_visual.points.color.a = 1.0;

  // Line strip 是蓝色
  area_visual.line_strip.color.b = 1.0;
  area_visual.line_strip.color.a = 1.0;

  // Line list 为红色
  area_visual.line_list.color.r = 1.0;
  area_visual.line_list.color.a = 1.0;


  //使用正弦和余弦生成螺旋结构。POINTS和LINE_STRIP markers都仅仅需要1个点作为每个顶点，然而LINE_LIST marker需要2个点 。
    geometry_msgs::Point p;
    for(int i=0;i<area_points_.size();i++)
    {
        p.x = area_points_[i].getX();
        p.y = area_points_[i].getY();
        p.z = 0;
        area_visual.points.points.push_back(p);
            // 点为绿色
            area_visual.points.color.g = 1.0f;
            area_visual.points.color.a = 1.0;

            // Line strip 是蓝色
            area_visual.line_strip.color.b = 1.0;
            area_visual.line_strip.color.a = 1.0;

            // Line list 为红色
            area_visual.line_list.color.r = 1.0;
            area_visual.line_list.color.a = 1.0;

        area_visual.line_strip.points.push_back(p);
    }

    p.x = area_points_[0].getX();
    p.y = area_points_[0].getY();
    p.z = 0;
    area_visual.line_strip.points.push_back(p);

    tf::Vector3 point_center;
    for(int i=0;i<hot_areas_.size();i++)
    {
         point_center = area2world_*hot_areas_[i].hot_point;
         p.x = point_center.getX();
         p.y = point_center.getY();
         p.z = 0;
         area_visual.points.points.push_back(p);
    }
    geometry_msgs::Point p1;


    for(int i=1;i<real_x_size_;i++)
    {
        point_center.setX(real_x_slice_*i);
        point_center.setY(0);
        point_center.setZ(1);
        point_center = area2world_*point_center;
        p1.x = point_center.getX();
        p1.y = point_center.getY();
        p1.z = 0;
        area_visual.line_list.points.push_back(p1);

        float length=sqrt((area_points_[3].getX()-area_points_[0].getX())*(area_points_[3].getX()-area_points_[0].getX())+
                (area_points_[3].getY()-area_points_[0].getY())*(area_points_[3].getY()-area_points_[0].getY()));
        point_center.setX(real_x_slice_*i);
        point_center.setY(length);
        point_center.setZ(1);
        point_center = area2world_*point_center;
        p1.x = point_center.getX();
        p1.y = point_center.getY();
        p1.z = 0;
        area_visual.line_list.points.push_back(p1);
    }

    for(int i=1;i<real_y_size_;i++)
    {
        point_center.setX(0);
        point_center.setY(real_y_slice_*i);
        point_center.setZ(1);
        point_center = area2world_*point_center;
        p1.x = point_center.getX();
        p1.y = point_center.getY();
        p1.z = 0;
        area_visual.line_list.points.push_back(p1);

        float length=sqrt((area_points_[1].getX()-area_points_[0].getX())*(area_points_[1].getX()-area_points_[0].getX())+
                (area_points_[1].getY()-area_points_[0].getY())*(area_points_[1].getY()-area_points_[0].getY()));
        point_center.setX(length);
        point_center.setY(real_y_slice_*i);
        point_center.setZ(1);
        point_center = area2world_*point_center;
        p1.x = point_center.getX();
        p1.y = point_center.getY();
        p1.z = 0;
        area_visual.line_list.points.push_back(p1);
    }
}
